#include "pid.h"


void PID_Init(PID_t* p, float kp, float ki, float kd, float ts,
              float out_min, float out_max, float integ_min, float integ_max)
{
    p->Kp = kp;p->Ki = ki;p->Kd = kd;
    p->Ts = ts;
    p->out_min = out_min; p->out_max = out_max;
    p->integ_min = integ_min; p->integ_max = integ_max;
    p->ref = p->meas = p->err = p->last_meas = 0.0f;
    p->integ = 0.0f; p->out = 0.0f;
}

float PID_Clamp(float x, float lo, float hi)
{ 
    if(x<lo) return lo; 
    if(x>hi) return hi;
    return x;
}

float PID_Update(PID_t* p, float ref, float meas)
{
    p->ref = ref;p->meas = meas;
    p->err = ref-meas;
    
    //Integration
    p->integ +=p->Ki*p->err*p->Ts;
    p->integ = PID_Clamp(p->integ,p->integ_min,p->integ_max);
    
    //Differentiation for Noise Resistance
    float d_meas = (meas - p->last_meas) / p->Ts;
    p->last_meas = meas;
    
    //Output
    float u = p->Kp * p->err + p->integ - p->Kd * d_meas;
    p->out = PID_Clamp(u, p->out_min, p->out_max);
    return p->out;
}
